#include "quadpod.fp.h"

#if defined(FIXED_KINEMATICS)
#include "quadpod_data.h"
#elif !defined(FIXMATH_NO_CACHE)
const run_t run[] __attribute__ ((section(".progmem")));
#endif


// *** Variable Declarations
// <<<
unsigned char sreg;
uint8_t	oop;
uint8_t step_last_known;

speed_t speed;

volatile ocr1a_t servo_cntrls;
volatile servo_cntrl_t servo;
volatile move_t move;
//>>>

#ifdef MATH
//<<<
angles_t	uhly;
#endif
//>>>

#ifdef USART_DEBUG
//<<<
uint8_t i;
uint8_t buffer[32];
uint8_t servo_trimming_index = 10;
uint8_t usart_manual_cntrl = 0;
volatile uint8_t usart_data;

/* USART Interrupt handler
 * -----------------------
 * This routine handles the interrupt flag "USART RX" signal.
 * Fetches the character, check the framing error
 * and evaluates the received byte. Either sends it 
 * to LCD for display or performs an LCD instruction
 */
#if defined( __AVR_ATmega8__ )
ISR(USART_RXC_vect)
{
	uint8_t c;

	//Store data to temp
	c = UDR;   
	// Check the framing error, defect frames are skipped and must be re-transmitted.
	if (bit_is_clear(UCSRA, FE)) {
		usart_data = c;
	} else {
		USART_send_str_P ( (uint8_t*) PSTR("Transmission Error\r\n"));
	}
}
#elif defined( __AVR_ATmega168__ )
ISR(USART_RX_vect)
{
	uint8_t c;

	//Store data to temp
	c = UDR0;   
	// Check the framing error, defect frames are skipped and must be re-transmitted.
	if (bit_is_clear(UCSR0A, FE0)) {
		usart_data = c;
	} else {
		USART_send_str_P ( (uint8_t*) PSTR("Transmission Error\r\n"));
	}
}
#else
#error "Unsupported AVR device. This code compiles for ATmega8, ATmega168 or ATmega328"
#endif

#endif //USART_DEBUG
//>>>

ISR(TIMER1_COMPA_vect)
/* Interrupt handler for compare match.  The controller fires this interrupt
 * every time the internal counter reaches the pre-set value (OCR1A). The
 * handler than sends the control signal to a selected servo by strobing the
 * clock signal on the servo shield bank. The time delay between tha last
 * strobe and the current one defines the length of the control pulse for a
 * servo. The handler manages bank switching in servo shield. Every bank
 * connects 6 servos only, the robot requires 12 servos to move.
 */
{ //<<<
	servo_cntrls.cntr += 1;
	servo_cntrls.cntr %= BANK_MAX_SIZE;

	if( 0 == servo_cntrls.cntr ) {
		servo_cntrls.bank ^= 1;
		if( servo_cntrls.bank ) {
			SERVOSHIELD_CTRL |= _BV(MR_BANK_1);
			SERVOSHIELD_CTRL &= ~_BV(MR_BANK_0);
		} else {
			SERVOSHIELD_CTRL |= _BV(MR_BANK_0);
			SERVOSHIELD_CTRL &= ~_BV(MR_BANK_1);
		}
	} 
	/* First task: swap the servo shield bank every 6 servos. 
	 * Each johnson counter control 6 servos only. 
	 */

	SERVOSHIELD_CTRL |= _BV(CP0);
	_delay_us(2);
#ifdef _TEST_MODE
	OCR1A = (uint16_t) NEUTRAL;
#else
	OCR1A = (uint16_t) (servo.current[servo_cntrls.servo_index] > 0 ? servo.current[servo_cntrls.servo_index] : NEUTRAL);
#endif
	servo_cntrls.servo_index += 1;
	servo_cntrls.servo_index %= COUNT_OF_SERVOS;
	if( 0 == servo_cntrls.servo_index ) {
		servo_cntrls.dirty ^= 1;
	}
	SERVOSHIELD_CTRL &= ~_BV(CP0);
	/* Second task: strobe the CP0 pin of 4017 for minimum 2us. 
	 * load the control byte from latch buffer into register OCR1A.
	 * Invalidate the latch buffer content after emptying latch buffer.
	 * by setting the "dirty" flag in servo_cntrls structure.
	 */

} //>>>

ISR(TIMER0_OVF_vect)
/* Interrupt handler for Timer0 overflow. This timer is used to define the 
 * timebase for derivation of the distance data deliverred by the SRF02 device. 
 * Derivation is used to determine the character of the obstacle detected by the
 * vision system of the robot. This handler trigers in fixed time intervals
 * (constant time base)
 *
 * The timer handler activates the routine for ranging.
 */
{	//<<<
}	//>>>

int main() // ******************** M A I N ****************************
{ //<<<
#ifdef USART_DEBUG
//<<<
	usart_data = 0x0;
#endif
//>>>

	init();
	fill_servo_page( (move_t) move );  							// Init the future structure
	fill_servo_page( (move_t) move );  							// Init the past structure
	fill_servo_buffer( (move_t) move, (speed_t) speed );  // Init the current structure

	_delay_ms(1000); // Hold on for 1 second to establish stable main

	servo_shield_port_init();		// Initialize HW for servo shield
	servo_shield_init();				// Initialize servo shield
	servo_shield_cntrl( ENABLE );	// enable servo shield control circuit

	wdt_enable(WDTO_1S); // restart the processor if something hangs for longer than 1 second.

#ifdef USART_DEBUG
//<<<
	USART_init();
	_delay_ms(200);
	USART_send_str_P( (uint8_t*) PSTR("START\r\n" ));
#endif
//>>>

	sei();

	while(1) {
#ifdef USART_DEBUG //<<<
		switch( usart_data )
		{
			case '?':
				USART_send_str_P( (uint8_t*) PSTR("Sending 24bytes of data."));
				break;

			case 'i':
#ifndef USART_DEBUG_SMALL_SIZE
				sprintf_P( (char*) buffer, PSTR("step=%d\r\n"), move.stepper_current );
				USART_send_str( (uint8_t*) buffer );
#else				
				USART_send_str_P( (uint8_t*) PSTR("step="));
				itoa( (int) move.stepper_current, (char*) buffer, 10 );
				USART_send_str( (uint8_t*) buffer );
				USART_send_str_P( (uint8_t*) PSTR("\r\n"));
#endif // USART_DEBUG_SMALL_SIZE

				for( i=0; i<COUNT_OF_SERVOS;i++ ) {
#ifndef USART_DEBUG_SMALL_SIZE
					sprintf_P( (char*) buffer, PSTR("Servo=%d,ORC1A=%d\r\n"), i, servo.current[i] );
					USART_send_str( (uint8_t*) buffer );
#else
					USART_send_str_P( (uint8_t*) PSTR("Servo="));
					itoa( (int) i, (char*) buffer, 10 );
					USART_send_str( (uint8_t*) buffer );
					USART_send_str_P( (uint8_t*) PSTR(",OCR1A="));
					itoa( (int) servo.current[i], (char*) buffer, 10 );
					USART_send_str( (uint8_t*) buffer );
					USART_send_str_P( (uint8_t*) PSTR("\r\n"));
#endif // USART_DEBUG_SMALL_SIZE
				}
				break;

			case 'N':
				servo_trimming_index += 1;
				servo_trimming_index %= COUNT_OF_SERVOS;
				USART_send_str_P( (uint8_t*) PSTR("trimmed servo="));
				itoa( (int) servo_trimming_index, (char*) buffer, 10 );
				USART_send_str( (uint8_t*) buffer );
				USART_send_str_P( (uint8_t*) PSTR("\r\n"));
				break;

			case '!':
				for( i=0; i<COUNT_OF_SERVOS;i++ ) {
					servo.current[i] = NEUTRAL;
				}
				break;

			case '+':
					servo.current[servo_trimming_index] += 6;
				break;

			case '-':
					servo.current[servo_trimming_index] -= 6;
				break;

			case 'm':
				usart_manual_cntrl ^= 1;
				break;
		
			case 'f':
				move.cntrl = FORWARD;
				break;
		
			case 'r':
				move.cntrl = REVERSE;
				break;
		
			case 's':
				move.cntrl = STOP;
				break;

			case 'C':
				move.movement += 1;
				move.movement %= 3;

			default:
				break;
		}

		usart_data = 0x0;
#endif
//>>>

		if( servo_cntrls.dirty ) {
			//<<<
			servo_cntrls.dirty ^= SIGNALED;
#ifdef USART_TRACING // <<<
			itoa( (int) speed.index, (char*) buffer, 10 );
			USART_send_str( (uint8_t*) buffer );
			USART_send_str_P( (uint8_t*) PSTR("->"));
#endif // >>>

			if( speed.index < speed.max ) {
				speed.index += 1;
				fill_servo_buffer( (move_t) move, (speed_t) speed );
			} else {
				if( move.cntrl != STOP ) {
					fill_servo_page( (move_t) move );
					speed.index = 0;
				}
			  	if( move.cntrl == REVERSE ) {
			  		if( move.stepper_current == 0 ) {
			  			move.stepper_current = STEP_TAB_SIZE;
			  		} 
			  	}
			  	move.stepper_current += move.cntrl; 
			  	move.stepper_current %= STEP_TAB_SIZE;

#ifdef USART_DEBUG // <<<
				if( 1 == usart_manual_cntrl ) {
					move.cntrl = STOP;
				}
#endif //>>>
			} 
		} //>>>

		wdt_reset();
		sleep_mode(); // may not be required, as the robot will repeatedly search for direction?
	}

	return 0;
} //>>>


void init( void ) // ********* F U N C T I O N S ********* 
{ //<<<
	servo_cntrls.cntr = 0;
	servo_cntrls.bank = 1;
	servo_cntrls.dirty = 0;
	servo_cntrls.servo_index = 0;
	step_last_known = 100; // INFINITY

	move.movement = STRAIGHT;
#ifdef USART_DEBUG
	move.cntrl = STOP;
#else
	move.cntrl = FORWARD;
#endif
	move.stepper_current = 0;
	move.stepper_previous = 100;

	speed.max = LINREG_NO_OF_STEPS;
	speed.index = speed.max;
} //>>>


void servo_shield_port_init()
{ //<<<
	SERVOSHIELD_DDR |= _BV(MR_BANK_0) | _BV(MR_BANK_1) | _BV(CP0);
/* Configure the data direction for servo port (B).
 * Pin MR (PB0) and PB1 (CP0) are configured for output
 */
} //>>>


void servo_shield_init( void )
{ //<<<
	unsigned char sreg;

//   TCCR1A |= _BV(COM1A0);
//   TCCR1A |= _BV(COM1A0) | _BV(COM1A1);
// configure "set OC1A output high on compare match"
	TCCR1A &= ~_BV(COM1A0);
	TCCR1A &= ~_BV(COM1A1);
// confiure OC1A output pins for normal operation

	TCCR1B = _BV(CS11) | _BV(WGM12);
/* CS11 - use CLK/8 prescale value
 * WGM1[3-0] = [0,1,0,0] = CTC mode.
 * Generate interrupt on T1 = OCR1A, set output pin high
 * and clear the counter. Start counting from zero until
 * the OCR1A value is reached again.
 */

	SERVOSHIELD_CTRL |= _BV(MR_BANK_0);
	SERVOSHIELD_CTRL |= _BV(MR_BANK_1);
	_delay_us(10);
	SERVOSHIELD_CTRL &= ~_BV(MR_BANK_0);
//	SERVOSHIELD_CTRL &= ~_BV(MR_BANK_1);
// Send 10us strobe to 4017 - decade counter - to reset
	 
	sreg = SREG;
	cli();
	
	TCNT1 = (uint16_t) 0;
	// Set the Timer/counter1 to zero

	OCR1A = (uint16_t) NEUTRAL;
	/* Set the compare register counter to prescalled value. 
	 *
	 * The compae match limit is set dynamically in interrupt 
	 * handler. It generates the control signals for attached
	 * servos.
	 */

	SREG = sreg;
} //>>>


void servo_shield_cntrl( type_t mode )
{ //<<<
	switch( mode ) 
	{
		case DISABLE:
#if defined( __AVR_ATmega8__ )
			TIMSK &= ~_BV(OCIE1A);
#elif defined( __AVR_ATmega168__ )
			TIMSK1 &= ~_BV(OCIE1A);
#else
#error "Unsupported AVR device. This code compiles for ATmega8, ATmega168 or ATmega328"
#endif
			// Disable interrupt called by compare match.
		break;

		case ENABLE:
#if defined( __AVR_ATmega8__ )
			TIMSK |= _BV(OCIE1A);
#elif defined( __AVR_ATmega168__ )
			TIMSK1 |= _BV(OCIE1A);
#else
#error "Unsupported AVR device. This code compiles for ATmega8, ATmega168 or ATmega328"
#endif
			// Enable interrupt called by compare match.
		break;
	}
} //>>>


void fill_servo_page( move_t m )
{ //<<<
	uint8_t i;
	uint8_t servo_latch_ptr;

	servo_latch_ptr = 10;

	sreg = SREG;
	cli();
	// Disable interrupts while writing the latch buffer. 
	
	for( i = 0; i < COUNT_OF_ARMS; i++ ) {
		servo_latch_ptr += 1;
		servo_latch_ptr %= COUNT_OF_SERVOS;
#ifdef _PROGMEM_DATA
		servo.past[servo_latch_ptr] = servo.future[servo_latch_ptr];
		servo.future[servo_latch_ptr] = (uint16_t) pgm_read_word( &run[m.movement].step[m.stepper_current].arm[i].servo_shoulder );
#else //<<<
		servo.past[servo_latch_ptr] = servo.future[servo_latch_ptr];
		eeprom_busy_wait();
		servo.future[servo_latch_ptr] = (uint16_t) eeprom_read_word( &run[m.movement].step[m.stepper_current].arm[i].servo_shoulder );
#endif
//>>>
		 
		servo_latch_ptr += 1;
		servo_latch_ptr %= COUNT_OF_SERVOS;

#ifdef _PROGMEM_DATA
		servo.past[servo_latch_ptr] = servo.future[servo_latch_ptr];
		servo.future[servo_latch_ptr] = (uint16_t) pgm_read_word( &run[m.movement].step[m.stepper_current].arm[i].servo_elbow );
#else //<<<
		servo.past[servo_latch_ptr] = servo.future[servo_latch_ptr];
		eeprom_busy_wait();
		servo.future[servo_latch_ptr] = (uint16_t) eeprom_read_word( &run[m.movement].step[m.stepper_current].arm[i].servo_elbow );
#endif
//>>>

		servo_latch_ptr += 1;
		servo_latch_ptr %= COUNT_OF_SERVOS;

#ifdef _PROGMEM_DATA
		servo.past[servo_latch_ptr] = servo.future[servo_latch_ptr];
		servo.future[servo_latch_ptr] = (uint16_t) pgm_read_word( &run[m.movement].step[m.stepper_current].arm[i].servo_anckle );
#else //<<<
		servo.past[servo_latch_ptr] = servo.future[servo_latch_ptr];
		eeprom_busy_wait();
		servo.future[servo_latch_ptr] = (uint16_t) eeprom_read_word( &run[m.movement].step[m.stepper_current].arm[i].servo_anckle );
#endif
//>>>

	}
	/* Fill the servo latch buffer. Apply rotation by 1 byte left, so that the slot No.12 contain
	 * the OCR1A data for first servo. The slot No.1 than contain value for second servo, etc.
	 */ 

	SREG = sreg;
} //>>>


void fill_servo_buffer( move_t m, speed_t s )
{ //<<<
	uint8_t i;
	int16_t temp;

	sreg = SREG;
	cli();
	// Disable interrupts while writing the latch buffer. 
	
	for( i = 0; i < COUNT_OF_SERVOS; i++ ) {
		temp = servo.future[i] - servo.past[i]; 
      temp *= s.index;
		temp /= s.max;
      temp += servo.past[i];
		servo.current[i] = (uint16_t) temp;
	}
	/* Fill the servo latch buffer based on the linear regression 
	 * beween two tabelated tabeleated entries. It already works
	 * with shifted buffer ( one byte left ).
	 */ 

	SREG = sreg; // enable interrupts again.
} //>>>
